#include <vector>
#include "zhnmat.hpp"
#include "utils.cpp"
using namespace std;
using namespace zhnmat;

int main()
{
    Mat P1(3, 3, vector<double>{404.9269208768735, 0, 336.058952331543, 0, 404.9269208768735, 240.7664337158203, 0, 0, 1});
    double TtimesF = 48722.26225851812;
    Mat p1inv = P1.inv();
    PRINT_NAME_VALUE(p1inv);
    Vector3d pixels[7];
    pixels[0] = Vector3d(445,728,130);
    pixels[1] = Vector3d(360,680,85);
    pixels[2] = Vector3d(316,756,254);
    pixels[3] = Vector3d(361,821,144);
    pixels[4] = Vector3d(507,888,125);
    pixels[5] = Vector3d(509,913,293);
    pixels[6] = Vector3d(478,859,339);
    for (int i=0; i<7; i++) {
        double xl = pixels[i]._x;
        double xr = pixels[i]._y - 640;
        double yl = pixels[i]._z;
        double Z = TtimesF / (xl - xr);
        Vector3d pixuv(xl, yl, 1);
        Vector3d ans = p1inv * pixuv * Z;
        PRINT_NAME_VALUE(ans);
  }
  return 0;
}
